
inline
emopt::emopt(const mat& in_data, const mat& in_means, const mat& in_covs, const vec& in_weights)
: data(in_data), means(in_means),covs (in_covs), weights(in_weights), 
Dim(in_data.n_rows), N_data(in_data.n_cols), N_cent(in_means.n_cols)
{
}

inline
void
emopt::run(const uword max_iter)
{
  
  for (uword i=0; i<max_iter; ++i)
  {
    //cout << "Doing emopt for iter = " << i << endl;
    // Calculating a posteriori probability
    calc_prob_aposteriori();
    // Calculating the effective number of data points
    Lg = eff_num(); // Calculating Lg
    
    means   = get_means();
    covs    = get_covs();
    weights = get_weights();
    //Lg.print("Lg: ");
    
  }
 
}

inline
void 
emopt::calc_prob_aposteriori() //log(2.26)
{
  prob_aposteriori = zeros(N_cent,N_data);
  mat log_prob_aposteriori = zeros(N_cent,N_data);
  double log_gauss_function;
  double log1;
  double log2;   // aposteriori_prob = log1-log2
  vec xn;
  rowvec log_prob_X = calc_log_prob_X();
  //cout << "log_prob_X: " << log_prob_X.t() << endl;
  
  for (uword gi=0; gi<N_cent; gi++)
  {
    for (uword  ni=0; ni<N_data ; ni++)
    {      
      xn=data.col(ni);
      log_gauss_function = calc_log_gauss_func(xn,gi);
      log1= log(weights(gi)) + log_gauss_function;
      log2=log_prob_X(ni);
      //cout << "log1: " << log1 << " log2: " << log2 << ". log1-log2: " << log1-log2 << endl;
      log_prob_aposteriori(gi,ni)=log1-log2;
    }
  }
  //log_prob_aposteriori.print("log_prob_aposteriori: ");
  //log_prob_aposteriori.save("log_prob_aposteriori.dat",raw_ascii);
  prob_aposteriori =exp(log_prob_aposteriori);
}

inline
double 
emopt::calc_log_gauss_func(const vec &xn, const uword g) //log(2.23)
{
  
  double logDetCov;
  double logDen;
  double logNum;
  double logGauss;
  //mat cov=zeros(Dim,Dim);
  vec res;
  vec mu;
  
  //cov.diag() = covs.col(g); 
  mu=means.col(g);
  //mat Xd = diagmat(covs.col(g));
  double logDetCov2;
  double sign;
  
  log_det(logDetCov2, sign, diagmat(covs.col(g)));
  
  //logDetCov = log(det( diagmat(covs.col(g)) ));
  
  //cout << "covs.col(g) : " << covs.col(g) << endl;
  //cout << "logDetCov: " << logDetCov << endl;
  //cout << "log_det(diagmat(covs.col(g)))= " << logDetCov2  << endl;
  //getchar();
  
  logDen=(-0.5)*(Dim*log(2*datum::pi) + logDetCov2);
  res = (xn - mu);
  logNum=as_scalar((-0.5)*res.t()*inv( diagmat(covs.col(g)) )*res);
  //cout << "logNum= "<< logNum  << ". logDen= " << logDen << endl;
  //getchar();
  logGauss = logNum + logDen;
  return logGauss;
}




inline
rowvec 
emopt::calc_log_prob_X()
{
  double log_gauss_function;
  double log_prob_xn;
  double tmp;
  rowvec log_prob_X;
  mat cov;
  vec mu;
  vec x;
  
  log_prob_X.zeros(N_data);
  
  for (uword ni = 0; ni<N_data ; ++ni)
  {
    // Calculating the probability
    //log_prob_xn = 0; ERROR - DON'T USE
    x=data.col(ni);
    
    for (uword gi=0; gi<N_cent; ++gi)
    {
      mu=means.col(gi);
      log_gauss_function=calc_log_gauss_func(x,gi);
      //cout << "log_gauss_function: " << log_gauss_function << ". ";

      if (gi == 0)
      {
	log_prob_xn =log(weights(gi))+log_gauss_function;
      }
      else
      {
	tmp = log(weights(gi))+log_gauss_function;
	log_prob_xn = log_add(log_prob_xn,tmp);
	//cout << "log_prob_xn: " << log_prob_xn << endl;
      }
    }
    log_prob_X(ni)=log_prob_xn;
  }
  return log_prob_X;
}

inline 
rowvec 
emopt::eff_num()
{
  rowvec effe_num;
  effe_num.zeros(N_cent);
  for (int gi=0; gi<N_cent; gi++)
  {
    effe_num(gi)=sum(prob_aposteriori.row(gi));
    //cout << "eff_g_" << gi << " = " << sum(prob_aposteriori.row(gi)) << ". "; 
    //prob_aposteriori.row(gi).print("prob_aposteriori: ");
    //getchar();
    //cout << "eff_g_" << gi << " = " << sum(prob_aposteriori.row(gi)) << ". ";
    //prob_aposteriori.row(gi).print("prob_aposteriori: ");

  }
  return effe_num;
}

inline
mat
emopt::get_means()
{
  // Updating Means
  mat new_means=zeros(Dim,N_cent);
  for (int gi=0; gi<N_cent; gi++)
  {
    for (int ni=0; ni<N_data ; ni++)
    {
      new_means.col(gi)+=prob_aposteriori(gi,ni)*data.col(ni);
    }
    new_means.col(gi)/=Lg(gi);
  }
  
  return new_means;
}


inline
mat
emopt::get_covs()
{
  vec res;
  mat new_covs=zeros(Dim, N_cent);
  //Updating Covariance Matrix
  mat cov = zeros(Dim,Dim);
  for (uword gi=0; gi<N_cent; ++gi)
  {
    for (uword ni=0; ni<N_data ; ++ni)
    {
      res = data.col(ni)-means.col(gi);
      cov+= prob_aposteriori(gi,ni)*res*res.t();
    }
    //cov.diag().t().print("cov.diag: ");
    cov/=Lg(gi);
    new_covs.col(gi)=cov.diag();
  }
  
  //new_covs;
  //new_covs.print("new_covs: ");
  return new_covs;
}



inline
vec
emopt::get_weights()
{
  vec new_weights = zeros(N_cent);
  for (uword gi=0; gi<N_cent; ++gi)
  {
    new_weights(gi)=Lg(gi)/double(N_data);
  }
  return new_weights;
}


